/*
Copyright (C) 2021-2025 Casa Xu (also Zhiyan Xu) from HIT

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

/*
 * LOS_CMOS.c
 *
 *  Created on: 2023年2月20日
 *      Author: 64435
 * 
 */

/* 小相机在轨工作方式 */
/* 地面发送小相机拍照指令后，小相机会成像并将照片存储在FLASH中
*  拍照后，OBC向小相机发送“发送最近一张照片指令”，小相机开始回传照片
*  OBC接收到数据后，将数据写入"1:cms.bin"文件中
*  "1:cms.bin"文件最多可存储200张照片，第0-1023字节保留，第1024-2047字节存储照片的信息
*  每条照片信息长度为5字节，第一字节为编号，范围是0-199，后四字节为照片大小（单位为字节）
*  第2048字节开始为照片，各照片间不设间隔，以照片信息为依据进行读写
*/

/* 小相机照片向地面传输方式 */

#include "LOS_CMOS.h"

uint8_t photo_buf[512];
uint8_t cmos_tmbuf[140];
uint8_t cmos_sdmsg[1024]; //存放SD卡中的照片信息
uint8_t cmos_p2gbuf[218]; //用于从测控下传照片的缓冲区
uint8_t pbready = 0;

extern FIL file_cmos;  //小相机照片从SD卡到测控专用
extern FIL file_tognd; //通过遥测转发
extern LOS_SemaphoreHandle_t BinSemphr_UP;

/** @fn void LOS_CMOS_Snap()
*   @brief 发送小相机拍照指令
*   @note 
*/
void LOS_CMOS_Snap()
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    SNP_CMD.SNP_dest = nID_CMOS;   
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0x0;                //内容识别码
    SNP_CMD.SNP_len = 10;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0X00), FP_CMD, 8);
    FP_CMD[0] = 0XF0;
    FP_CMD[1] = 0X01;
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0XFF), FP_CMD, 2);
}

/** @fn void LOS_CMOS_SendRecentPhoto()
*   @brief 发送最近一张照片指令
*   @note 该指令执行后，LOS_TM_CMOSTRAN会被赋值为600，LOS_TM_CMOSTRAN会在遥测线程中没0.1s减小1直至0，期间综电会停止与测控板的通信，LOS_TM_CMOSTRAN为0时恢复
*/
void LOS_CMOS_SendRecentPhoto()
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    SNP_CMD.SNP_dest = nID_CMOS;   
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0x0;                //内容识别码
    SNP_CMD.SNP_len = 10;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0X00), FP_CMD, 8);
    FP_CMD[0] = 0XF0;
    FP_CMD[1] = 0X02;
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0XFF), FP_CMD, 2);
    // if (STA_CMOS.CMD == 1)
    // {
    //     LOS_TM_CMOSTRAN = 600;
    // }
}


/** @fn void LOS_CMOS_SendRecentPhotoMsg()
*   @brief 发送最近一张照片编号与长度
*   @note 
*/
void LOS_CMOS_SendRecentPhotoMsg()
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    SNP_CMD.SNP_dest = nID_CMOS;   
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0x0;                //内容识别码
    SNP_CMD.SNP_len = 10;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0X00), FP_CMD, 8);
    FP_CMD[0] = 0XF0;
    FP_CMD[1] = 0X03;
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0XFF), FP_CMD, 2);
    CMOS_NOWSIZE = 0;
}


/** @fn void LOS_CMOS_STA()
*   @brief 发送本机工程参数
*   @note 
*/
void LOS_CMOS_STA()
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    SNP_CMD.SNP_dest = nID_CMOS;   
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0x0;                //内容识别码
    SNP_CMD.SNP_len = 10;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0X00), FP_CMD, 8);
    FP_CMD[0] = 0XF0;
    FP_CMD[1] = 0X04;
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0XFF), FP_CMD, 2);
}


/** @fn void LOS_CMOS_JPEG()
*   @brief 直接拍照并返回原始JPEG数据
*   @note 
*/
void LOS_CMOS_JPEG()
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    SNP_CMD.SNP_dest = nID_CMOS;   
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0x0;                //内容识别码
    SNP_CMD.SNP_len = 10;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0X00), FP_CMD, 8);
    FP_CMD[0] = 0XF0;
    FP_CMD[1] = 0X05;
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0XFF), FP_CMD, 2);
}



/** @fn void LOS_CMOS_FormatFlash()
*   @brief 重新格式化Flash
*   @note 
*/
void LOS_CMOS_FormatFlash()
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    SNP_CMD.SNP_dest = nID_CMOS;   
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0x0;                //内容识别码
    SNP_CMD.SNP_len = 10;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0X00), FP_CMD, 8);
    FP_CMD[0] = 0XF0;
    FP_CMD[1] = 0X06;
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0XFF), FP_CMD, 2);
}


/** @fn void LOS_CMOS_AllPhotoMsg()
*   @brief 发送全部照片状态
*   @note 
*/
void LOS_CMOS_AllPhotoMsg()
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    SNP_CMD.SNP_dest = nID_CMOS;   
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0x0;                //内容识别码
    SNP_CMD.SNP_len = 10;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0X00), FP_CMD, 8);
    FP_CMD[0] = 0XF0;
    FP_CMD[1] = 0X07;
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0XFF), FP_CMD, 2);
}


/** @fn void LOS_CMOS_DefaultArg()
*   @brief 设置当前拍照参数为默认参数
*   @note 
*/
void LOS_CMOS_DefaultArg()
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    SNP_CMD.SNP_dest = nID_CMOS;   
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0x0;                //内容识别码
    SNP_CMD.SNP_len = 10;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0X00), FP_CMD, 8);
    FP_CMD[0] = 0XF0;
    FP_CMD[1] = 0X08;
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0XFF), FP_CMD, 2);
}


/** @fn void LOS_CMOS_CheckArg()
*   @brief 发送当前FLASH拍照参数
*   @note 
*/
void LOS_CMOS_CheckArg()
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    SNP_CMD.SNP_dest = nID_CMOS;   
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0x0;                //内容识别码
    SNP_CMD.SNP_len = 10;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0X00), FP_CMD, 8);
    FP_CMD[0] = 0XF0;
    FP_CMD[1] = 0X09;
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0XFF), FP_CMD, 2);
}

/** @fn void LOS_CMOS_ReadPhoto(uint32_t photoid, uint32_t offset, uint32_t length)
*   @brief 照片读取
*   @param[in] photoid 照片编号
*   @param[in] offset 起始偏移（实际只使用低24位）
*   @param[in] length 数据长度（实际只使用低24位）
*   @note 
*/
void LOS_CMOS_ReadPhoto(uint32_t photoid, uint32_t offset, uint32_t length)
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    uint8_t bytebuf[4];
    uint32_t u32buf = photoid;
    SNP_CMD.SNP_dest = nID_CMOS;   
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0x0;                //内容识别码
    SNP_CMD.SNP_len = 20;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0X00), FP_CMD, 8);
    FP_CMD[0] = 0XF1;
    FP_CMD[1] = 0X0A;
    memcpy(&FP_CMD[2], &u32buf, 4);
    u32buf = offset;
    memcpy(bytebuf, &u32buf, 4);
    memcpy(&FP_CMD[6], &bytebuf[1], 2);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0X01), FP_CMD, 8);
    FP_CMD[0] = bytebuf[3];
    u32buf = length;
    memcpy(bytebuf, &u32buf, 4);
    memcpy(&FP_CMD[1], &bytebuf[1], 3);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0XFF), FP_CMD, 4);
}

/** @fn void LOS_CMOS_PhotoLength(uint8_t photoid)
*   @brief 请求指定照片的长度
*   @param[in] photoid 照片编号
*   @note 
*/
void LOS_CMOS_PhotoLength(uint8_t photoid)
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    uint8_t bytebuf[4];
    uint32_t u32buf = photoid;
    SNP_CMD.SNP_dest = nID_CMOS;   
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0x0;                //内容识别码
    SNP_CMD.SNP_len = 11;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0X00), FP_CMD, 8);
    FP_CMD[0] = 0XF3;
    FP_CMD[1] = 0X0C;
    FP_CMD[2] = photoid;
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0XFF), FP_CMD, 3);
}

/** @fn void LOS_CMOS_PhotoArg(uint8_t photoid)
*   @brief 请求指定照片的参数
*   @param[in] photoid 照片编号
*   @note 
*/
void LOS_CMOS_PhotoArg(uint8_t photoid)
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    uint8_t bytebuf[4];
    uint32_t u32buf = photoid;
    SNP_CMD.SNP_dest = nID_CMOS;   
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0x0;                //内容识别码
    SNP_CMD.SNP_len = 11;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0X00), FP_CMD, 8);
    FP_CMD[0] = 0XF4;
    FP_CMD[1] = 0X0D;
    FP_CMD[2] = photoid;
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0XFF), FP_CMD, 3);
}

/** @fn void LOS_CMOS_SetArg()
*   @brief 设置拍照参数
*   @note 
*/
void LOS_CMOS_SetArg(uint8_t bright, uint8_t contrast, uint8_t sharp, uint8_t ev, uint8_t compress,uint8_t color)
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    SNP_CMD.SNP_dest = nID_CMOS;   
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0x0;                //内容识别码
    SNP_CMD.SNP_len = 19;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0X00), FP_CMD, 8);
    FP_CMD[0] = 0XF2;
    FP_CMD[1] = 0X0B;
    FP_CMD[2] = 0X02;
    FP_CMD[3] = bright;
    FP_CMD[4] = contrast;
    FP_CMD[5] = sharp;
    FP_CMD[6] = ev;
    FP_CMD[7] = compress;
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0X01), FP_CMD, 8);
    FP_CMD[0] = color;
    FP_CMD[1] = 0X01;
    FP_CMD[2] = 0XFF;
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_CMOS, 0XFF), FP_CMD, 3);
}

/** @fn void LOS_CMOS_FileInit()
*   @brief 初始化照片存储文件
*   @note 
*/
void LOS_CMOS_FileInit()
{
    uint32_t bhw = 0;
    FRESULT res_sd; 
    uint8_t errcnt = 0;
    uint8_t cnt = 0;
    do
    {
        res_sd = f_open(&file_cmos, "1:cms.bin", FA_OPEN_ALWAYS | FA_WRITE | FA_READ);
        errcnt++;
    }while(res_sd!=FR_OK && (errcnt < 10));
    errcnt = 0;

    if (res_sd == FR_OK) 
    {
        f_lseek(&file_cmos,2048);  //文件有可能刚被创建，先开辟2048字节的空间
        f_lseek(&file_cmos,1024);  //回来读照片信息
        do 
        {
            res_sd = f_read(&file_cmos, cmos_sdmsg, 1024, &bhw); //读了1024个字节的信息
            errcnt++;
        }while(res_sd!=FR_OK && (errcnt < 10));

        for (cnt=0;cnt<200;cnt++)
        {
            if (cmos_sdmsg[(uint32_t)cnt*5] != cnt) //要么是没有信息，要么是信息被损坏
            {
                cmos_sdmsg[(uint32_t)cnt*5] = cnt;   //补充或修复
                cmos_sdmsg[(uint32_t)cnt*5 + 1] = 0;
                cmos_sdmsg[(uint32_t)cnt*5 + 2] = 0;
                cmos_sdmsg[(uint32_t)cnt*5 + 3] = 0;
                cmos_sdmsg[(uint32_t)cnt*5 + 4] = 0;
            }
        }

        f_lseek(&file_cmos,1024);  //把照片信息写回去
        do
        {
            res_sd = f_write(&file_cmos, cmos_sdmsg, 1024, &bhw);
            errcnt++;
        } while (res_sd!=FR_OK && (errcnt < 10));
        f_close(&file_cmos);
    }
    else
    {
        f_close(&file_cmos);
    }
}

/** @fn void LOS_CMOS_FileReInit()
*   @brief 重新初始化照片存储文件
*   @note 该函数与上一个函数不同，这个函数会直接重新覆写照片信息区
*/
void LOS_CMOS_FileReInit()
{
    uint32_t bhw = 0;
    FRESULT res_sd; 
    uint8_t errcnt = 0;
    uint8_t cnt = 0;
    do
    {
        res_sd = f_open(&file_cmos, "1:cms.bin", FA_CREATE_ALWAYS | FA_WRITE | FA_READ);
        errcnt++;
    }while(res_sd!=FR_OK && (errcnt < 10));
    errcnt = 0;

    if (res_sd == FR_OK) 
    {
        f_lseek(&file_cmos,2048);  //文件有可能刚被创建，先开辟2048字节的空间

        for (cnt=0;cnt<200;cnt++)
        {
            cmos_sdmsg[(uint32_t)cnt*5] = cnt;   
            cmos_sdmsg[(uint32_t)cnt*5 + 1] = 0;
            cmos_sdmsg[(uint32_t)cnt*5 + 2] = 0;
            cmos_sdmsg[(uint32_t)cnt*5 + 3] = 0;
            cmos_sdmsg[(uint32_t)cnt*5 + 4] = 0;
        }

        f_lseek(&file_cmos,1024);  //把照片信息写回去
        do
        {
            res_sd = f_write(&file_cmos, cmos_sdmsg, 1024, &bhw);
            errcnt++;
        } while (res_sd!=FR_OK && (errcnt < 10));
        f_close(&file_cmos);
    }
    else
    {
        f_close(&file_cmos);
    }
}

/** @fn uint8_t LOS_CMOS_GetSDPicNum(uint32_t* totalsize)
*   @brief 获取SD卡里存放的照片数量
*   @param totalsize SD卡中存放照片的总大小会通过该值传递
*   @return SD卡里已经存放的照片数量
*   @note 调用该函数后，cmos_sdmsg中的信息也会更新一次
*/
uint8_t LOS_CMOS_GetSDPicNum(uint32_t* totalsize)
{
    uint32_t bhw = 0;
    FRESULT res_sd; 
    uint8_t errcnt = 0;
    uint8_t cnt = 0;
    uint32_t length;
    *totalsize = 0;
    do
    {
        res_sd = f_open(&file_cmos, "1:cms.bin", FA_OPEN_ALWAYS | FA_READ);
        errcnt++;
    }while(res_sd!=FR_OK && (errcnt < 10));
    errcnt = 0;    
    if (res_sd == FR_OK)
    {
        f_lseek(&file_cmos,1024);  
        do 
        {
            res_sd = f_read(&file_cmos, cmos_sdmsg, 1024, &bhw); //读了1024个字节的信息
            errcnt++;
        }while(res_sd!=FR_OK && (errcnt < 10));
        f_close(&file_cmos);

        for(cnt=0;cnt<200;cnt++)
        {
            length = 0;
            memcpy(&length, &cmos_sdmsg[(uint32_t)cnt*5 + 1], 4);
            *totalsize += length;
            if (length == 0)
            {
                break;
            }
        }

        return cnt;
    }
    f_close(&file_cmos);
    return 0;
}

/** @fn void LOS_CMOS_UpdateSDMSG(uint8_t picnum, uint32_t length)
*   @brief 更新SD卡中的照片信息
*   @param picnum 要改第几张照片的长度信息
*   @param length 照片的长度
*   @note 向SD卡写完一张照片后，应调用该函数更新照片信息
*/
void LOS_CMOS_UpdateSDMSG(uint8_t picnum, uint32_t length)
{
    uint32_t bhw = 0;
    FRESULT res_sd; 
    uint8_t errcnt = 0;
    uint8_t cnt = 0;
    uint32_t len_cpy = length;
    do
    {
        res_sd = f_open(&file_cmos, "1:cms.bin", FA_OPEN_ALWAYS | FA_WRITE | FA_READ);
        errcnt++;
    }while(res_sd!=FR_OK && (errcnt < 10));
    errcnt = 0;

    if (res_sd == FR_OK) 
    {
        f_lseek(&file_cmos,2048);  //文件有可能刚被创建，先开辟2048字节的空间
        f_lseek(&file_cmos,1024);  //回来读照片信息
        do 
        {
            res_sd = f_read(&file_cmos, cmos_sdmsg, 1024, &bhw); //读了1024个字节的信息
            errcnt++;
        }while(res_sd!=FR_OK && (errcnt < 10));

        memcpy(&cmos_sdmsg[(uint32_t)picnum*5+1], &len_cpy, 4);

        f_lseek(&file_cmos,1024);  //把照片信息写回去
        do
        {
            res_sd = f_write(&file_cmos, cmos_sdmsg, 1024, &bhw);
            errcnt++;
        } while (res_sd!=FR_OK && (errcnt < 10));
        f_close(&file_cmos);
    }
    else
    {
        f_close(&file_cmos);
    }
}

/** @fn void LOS_CMOS_Pic2EarthPack(uint8_t picnum)
*   @brief 
*   @param picnum 要传SD卡中的第几张照片
*   @param pbuf   照片传输包的指针
*   @note 该函数把SD卡中的指定照片打包成一个个遥测包
*/
int8_t LOS_CMOS_Pic2EarthPack(uint8_t picnum, uint8_t* pbuf)
{
    static uint32_t packcnt = 0; //包计数，记录传到了第几包
    static uint32_t piclen = 0;  //一开始是照片长度，然后是照片包数
    static uint32_t lenbuf = 0;
    static uint32_t totallen = 0; //前面的照片的总长度
    static uint32_t bhr = 0;             //FATFS读操作需要，表示读了多少

    uint8_t FATFS_Result = 0;    //FATFS操作返回值，用于判断FATFS操作是否成功。
    uint32_t fcnt = 0;           //FATFS操作计数器
    uint32_t cnt = 0;            //for循环用的计数器

    STA_CMOS.piccnt = LOS_CMOS_GetSDPicNum(&STA_CMOS.pictotalsize); //更新一下内存中的照片存储信息

    if (packcnt == 0) //第一次进来先获取一下之前照片的总长度
    {
        for (cnt=0;cnt<picnum;cnt++)
        {
            memcpy(&lenbuf, &cmos_sdmsg[cnt*5 + 1], 4);
            totallen += lenbuf;
        }
        memcpy(&piclen, &cmos_sdmsg[picnum*5 + 1], 4);
        piclen = piclen/256;
    }

    if (piclen != 0) //只有长度不为0的时候，才打包传输
    {
        do
        {
            FATFS_Result = f_open(&file_tognd, "1:cms.bin", FA_OPEN_EXISTING | FA_READ);    //先打开文件
            fcnt++;
        } while ((FATFS_Result != FR_OK) && (fcnt < 40));
        fcnt = 0;

        if (FATFS_Result == FR_OK)
        {
            f_lseek(&file_tognd, (2048 + totallen + 6 + (packcnt*256)));
            f_read(&file_tognd, pbuf, 218, &bhr);
            f_close(&file_tognd);
            packcnt++;
            piclen--;
            LOS_CMOS_OBC2TCLEFT = piclen;
            if (piclen>0)
            {
                return 1; //还没传输完
            }
            else
            {
                piclen = 0;   //还是清0一下，以防万一
                packcnt = 0;
                totallen = 0;
                lenbuf = 0;
                return 0; //传输完了
            }
        }
        else
        {
            f_close(&file_tognd);
            piclen = 0;   //还是清0一下，以防万一
            LOS_CMOS_OBC2TCLEFT = piclen;
            packcnt = 0;
            totallen = 0;
            lenbuf = 0;
            return -1;  //文件系统异常
        }
    }
    else //长度为0，照片不存在或者已经传输完了，不再打包
    {
        piclen = 0;   //还是清0一下，以防万一
        LOS_CMOS_OBC2TCLEFT = piclen;
        packcnt = 0;
        totallen = 0;
        lenbuf = 0;
        return 0; //传输完了
    }
    //不应该运行到这里
}


void LOS_CAN_CMOSCallBack(LOS_CANMSG_t* can_re)
{
    //首部校验用法：首部校验通过/未通过会改变canerrflag的值，所有消息会被先存在缓冲区，canerrflag有效时才会转存，无效时舍弃
    uint8_t id_filter[4];
    static uint8_t rec = 0;
    static uint8_t rec_pic = 0; //用于去区分回传的照片是最近一张照片还是指定长度照片
    uint32_t index = 0;
    static uint8_t packcnt = 0;
    static uint8_t piclenbuf[4]; 
    static uint8_t pic_framecnt = 0;        //帧计数器，用来数有没有漏帧

    BaseType_t xHigherPriorityTaskWoken;   //RTOS信号量用，别动

    memcpy(id_filter, &(can_re->ID), 4);   //将帧ID分为4个字节存储，便于处理
    if (id_filter[3]==0x00)                //若为首帧
    {
        rec = can_re->msg[6];

        if (rec == 0xF0) //如果是照片，则准备存储
        {
            //CMOS_NOWSIZE = 0;
            //CMOS_LASTFRAME = 0x55;
            //CMOS_PICCNT = LOS_CMOS_GetSDPicNum(&CMOS_TOTALSIZE); //获取一下已经存的数量和大小
        }
    }
    else if (id_filter[3] == 0xFF)
    {
        if (rec == 0xF0)
        {
            if (rec_pic == 0x80)
            {
                if ((packcnt % 2) == 0)
                {
                    memcpy(&photo_buf[252],can_re->msg,4);
                    pbready++;
                    LOS_HFT_STA = 1;
                    //CMOS_LASTFRAME = 0XAA;
                    packcnt++;
                }
                else 
                {
                    memcpy(&photo_buf[252+256],can_re->msg,4);
                    pbready++;
                    LOS_HFT_STA = 1;
                    //CMOS_LASTFRAME = 0XAA;
                    packcnt++;
                }
            }
            else if (rec_pic == 0x81)
            {
                memcpy(&STA_CMOS.picbuf[252],can_re->msg,4);
                pic_framecnt++;
                if (pic_framecnt != 34)
                {
                    //漏帧，啥也不是
                }
                else
                {
                    pic_framecnt = 0;
                    LOS_BinSemphr_GiveFromISR(BinSemphr_UP, &xHigherPriorityTaskWoken);
                }
            }
    
        }
        else if (rec == 0xF1) //最近一张照片编号和长度
        {
            memcpy(&cmos_tmbuf[6], &can_re->msg[0], 1);
            piclenbuf[3] = can_re->msg[0];
            memcpy(&STA_CMOS.lastpiclen, piclenbuf, 4);
        }
        else if (rec == 0xF2) //本机工参
        {
            memcpy(&cmos_tmbuf[7], &can_re->msg[2], 5);
        }
        else if (rec == 0xF3) //全部照片状态 100字节
        {
            memcpy(&cmos_tmbuf[108], &can_re->msg[0], 4);
        }
        else if (rec == 0xF4) //指定照片长度
        {
            memcpy(&cmos_tmbuf[118], &can_re->msg[0], 1);
            piclenbuf[3] = can_re->msg[0];
            memcpy(&STA_CMOS.lastpiclen, piclenbuf, 4);
            if (STA_CMOS.readsta == 1)
            {
                STA_CMOS.readsta = 2;
            }
        }
        else if (rec == 0xF5) //指定照片参数
        {
            memcpy(&cmos_tmbuf[125], &can_re->msg[0], 3);
        }
        else if (rec == 0xF6) //当前拍照参数
        {
            STA_CMOS.color = can_re->msg[0];
            memcpy(&cmos_tmbuf[134], &can_re->msg[0], 2);
        }
    }
    else
    {
        if (rec == 0xF0)
        {
            if (id_filter[3] != 0x01)
            {
                if (rec_pic == 0x80) //最近一张照片
                {
                    if ((packcnt % 2) == 0)
                    {
                        if (id_filter[3]==0x02)
                        {
                            memcpy(&photo_buf[0], &can_re->msg[4], 4);
                        }
                        else
                        {
                            index = 4 + (id_filter[3]-3)*8;
                            memcpy(&photo_buf[index], can_re->msg, 8);
                        }
                    }
                    else
                    {
                        if (id_filter[3]==0x02)
                        {
                            memcpy(&photo_buf[0+256], &can_re->msg[4], 4);
                        }
                        else
                        {
                            index = 4 + (id_filter[3]-3)*8;
                            memcpy(&photo_buf[index+256], can_re->msg, 8);
                        }
                    }
                }
                else if (rec_pic == 0x81) //请求指定照片
                {
                    if (id_filter[3] == 0x02)
                    {
                        piclenbuf[3] = can_re->msg[0];
                        memcpy(&STA_CMOS.offset, piclenbuf, 4); //偏移地址
                        piclenbuf[0] = 0;
                        piclenbuf[1] = can_re->msg[1];
                        piclenbuf[2] = can_re->msg[2];
                        piclenbuf[3] = can_re->msg[3];
                        memcpy(&STA_CMOS.size, piclenbuf, 4);  //数据长度
                        memcpy(STA_CMOS.picbuf, &can_re->msg[4], 4);
                        pic_framecnt = 2;
                    }
                    else
                    {
                        index = 4 + (id_filter[3]-3)*8;                            
                        memcpy(&STA_CMOS.picbuf[index], can_re->msg, 8);
                        pic_framecnt++;
                    }
                }

            }
            else //如果是01帧
            {
                rec_pic = can_re->msg[0];
                memcpy(&STA_CMOS.picid, &can_re->msg[2], 4);
                piclenbuf[0] = 0;
                memcpy(&piclenbuf[1], &can_re->msg[6], 2);
            }
        }
        else if (rec == 0xF1) //最近一张照片编号和长度
        {
            memcpy(&cmos_tmbuf[0], &can_re->msg[2], 6);
            memcpy(&STA_CMOS.lastpicid, &can_re->msg[2], 4);
            piclenbuf[1] = can_re->msg[6];
            piclenbuf[2] = can_re->msg[7];
        }
        else if (rec == 0xF3) //全部照片状态 100字节 13帧（算首部14帧）
        {
            memcpy(&cmos_tmbuf[12 + (id_filter[3] - 1) * 8], &can_re->msg[0], 8);
        }
        else if (rec == 0xF4) //指定照片长度
        {
            memcpy(&cmos_tmbuf[112], &can_re->msg[2], 6);
            memcpy(&STA_CMOS.lastpicid, &can_re->msg[2], 4);
            piclenbuf[1] = can_re->msg[6];
            piclenbuf[2] = can_re->msg[7];
        }
        else if (rec == 0xF5) //指定照片参数
        {
            memcpy(&cmos_tmbuf[119], &can_re->msg[2], 6);
        }
        else if (rec == 0xF6) //当前拍照参数
        {
            STA_CMOS.bright = can_re->msg[3];
            STA_CMOS.contrast = can_re->msg[4];
            STA_CMOS.sharp = can_re->msg[5];
            STA_CMOS.ev = can_re->msg[6];
            STA_CMOS.compress = can_re->msg[7];
            memcpy(&cmos_tmbuf[128], &can_re->msg[2], 6);
        }
    }
}

void LOS_CMOS_PWROn()
{
    LOS_EPS_Switch(19, SWITCH_ON);
    STA_CMOS.CMD = 1;
}

void LOS_CMOS_PWROff()
{
    LOS_EPS_Switch(19, SWITCH_OFF);
    STA_CMOS.CMD = 0;
}
